#include "System.h"

Incremental_pid  pid_speed3;
Incremental_pid  pid_speed4;
Incremental_pid  pid_speed5;
Incremental_pid  pid_speed8;


/**
  * @brief  PID参数初始化
	*	@note
  * @retval
  */
void PID_param_init()
{
    pid_speed3.err=0.0;
	pid_speed3.actual_val=0.0;
    pid_speed3.err_last=0.0;
    pid_speed3.err_next=0.0;
	pid_speed3.integral=0.0;
	 pid_speed3.Kp = 150;
	 pid_speed3.Ki = 0.2;
	 pid_speed3.Kd = 80;
	
    pid_speed4.err=0.0;
	pid_speed4.actual_val=0.0;
    pid_speed4.err_last=0.0;
    pid_speed4.err_next=0.0;
	pid_speed4.integral=0.0;
	pid_speed4.Kp = 150;
	pid_speed4.Ki = 0.2;
	pid_speed4.Kd = 80;
	
    pid_speed5.err=0.0;
    pid_speed5.err_last=0.0;
	pid_speed5.actual_val=0.0;
    pid_speed5.err_next=0.0;
	pid_speed5.integral=0.0;	
	pid_speed5.Kp = 150;
	pid_speed5.Ki = 0.2;
	pid_speed5.Kd = 80;
	
    pid_speed8.err=0.0;
    pid_speed8.err_last=0.0;
	pid_speed8.actual_val=0.0;
    pid_speed8.err_next=0.0;
	pid_speed8.integral=0.0;
	pid_speed8.Kp = 150;
	pid_speed8.Ki = 0.2;
	pid_speed8.Kd = 80;
#if 1
    float pid_temp3[3] = {pid_speed3.Kp, pid_speed3.Ki, pid_speed3.Kd};
	float pid_temp4[3] = {pid_speed4.Kp, pid_speed4.Ki, pid_speed4.Kd};
	float pid_temp5[3] = {pid_speed5.Kp, pid_speed5.Ki, pid_speed5.Kd};
	float pid_temp8[3] = {pid_speed8.Kp, pid_speed8.Ki, pid_speed8.Kd};
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH1, pid_temp3, 3);
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH2, pid_temp4, 3);
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH3, pid_temp5, 3);
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH4, pid_temp8, 3);

#endif
}


void set_p_i_d(Incremental_pid *pid, float p, float i, float d)
{
  	    pid->Kp = p;
		pid->Ki = i;
		pid->Kd = d;
}
/**
  * @brief  PID
  * @param  actual_val:ֵ
	*	@note
  * @retval
  */
#define SPE_DEAD_ZONE 2.0f /*速度环死区*/
#define SPE_INTEGRAL_START_ERR 50 /*积分分离时对应的误差范围*/
#define SPE_INTEGRAL_MAX_VAL 1000   /*积分范围限定，防止积分饱和*/

float speed_pid_realize(Incremental_pid *pid,float Encoder,float Target)
{
		/*计算目标值与实际值的误差*/
    pid->err=Target-Encoder;


    if((pid->err<2.0f )&& (pid->err>-2.0f))
	{
      pid->err = 0.0f;
	  pid->integral = 0;
      pid->err_last = 0;
	}
#if 1
    /*积分项*/
	pid->integral += pid->err;
#else
    /*积分项，积分分离，偏差较大时去掉积分作用*/
    if(pid->err > -SPE_INTEGRAL_START_ERR && pid->err < SPE_INTEGRAL_START_ERR)
    {
        pid->integral += pid->err;
        /*积分范围限定，防止积分饱和*/
        if(pid->integral > SPE_INTEGRAL_MAX_VAL)
        {
            pid->integral = SPE_INTEGRAL_MAX_VAL;
        }
        else if(pid->integral < -SPE_INTEGRAL_MAX_VAL)
        {
            pid->integral = -SPE_INTEGRAL_MAX_VAL;
        }
    }
#endif
		/*PID算法实现*/
    pid->actual_val = pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);
  
		/*误差传递*/
    pid->err_last=pid->err;
    
		/*返回当前实际值*/
    return pid->actual_val;
}



